/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file ImuFactorsExample
 * @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor
 * navigation code.
 * @author Garrett (ghemann@gmail.com), Luca Carlone
 */

/**
 * Example of use of the imuFactors (imuFactor and combinedImuFactor) in
 * conjunction with GPS
 *  - imuFactor is used by default. You can test combinedImuFactor by
 *  appending a `-c` flag at the end (see below for example command).
 *  - we read IMU and GPS data from a CSV file, with the following format:
 *  A row starting with "i" is the first initial position formatted with
 *  N, E, D, qx, qY, qZ, qW, velN, velE, velD
 *  A row starting with "0" is an imu measurement
 *  (body frame - Forward, Right, Down)
 *  linAccX, linAccY, linAccZ, angVelX, angVelY, angVelX
 *  A row starting with "1" is a gps correction formatted with
 *  N, E, D, qX, qY, qZ, qW
 * Note that for GPS correction, we're only using the position not the
 * rotation. The rotation is provided in the file for ground truth comparison.
 *
 *  See usage: ./ImuFactorsExample --help
 */

#include <boost/program_options.hpp>

// GTSAM related includes.
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/dataset.h>

#include <cstring>
#include <fstream>
#include <iostream>

using namespace gtsam;
using namespace std;

using symbol_shorthand::B;  // Bias  (ax,ay,az,gx,gy,gz)
using symbol_shorthand::V;  // Vel   (xdot,ydot,zdot)
using symbol_shorthand::X;  // Pose3 (x,y,z,r,p,y)

namespace po = boost::program_options;

po::variables_map parseOptions(int argc, char* argv[]) 
{
  po::options_description desc;
  desc.add_options()("help,h", "produce help message")(
      "data_csv_path", po::value<string>()->default_value("imuAndGPSdata.csv"),
      "path to the CSV file with the IMU data")(
      "output_filename",
      po::value<string>()->default_value("imuFactorExampleResults.csv"),
      "path to the result file to use")("use_isam", po::bool_switch(),
                                        "use ISAM as the optimizer");

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);

  if (vm.count("help")) {
    cout << desc << "\n";
    exit(1);
  }

  return vm;
}

// 创建并返回一个配置了IMU（惯性测量单元）噪声模型的参数对象
boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() 
{
  // 定义噪声参数
  // We use the sensor specs to build the noise model for the IMU factor.
  double accel_noise_sigma = 0.0003924; // 加速度计噪声标准差
  double gyro_noise_sigma = 0.000205689024915; // 陀螺仪的噪声标准差
  double accel_bias_rw_sigma = 0.004905; // 加速度零偏标准差
  double gyro_bias_rw_sigma = 0.000001454441043; // 陀螺仪零偏标准差
  // 创建噪声协方差矩阵
  Matrix33 measured_acc_cov = I_3x3 * pow(accel_noise_sigma, 2); // 加速度计噪声标准差计算出的噪声协方差矩阵
  Matrix33 measured_omega_cov = I_3x3 * pow(gyro_noise_sigma, 2);// 陀螺仪的噪声标准差计算出的噪声协方差矩阵

  // 表示从速度积分到位置时的积分误差协方差矩阵，这里假设了一个很小的值
  Matrix33 integration_error_cov = I_3x3 * 1e-8;  // error committed in integrating position from velocities

  Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2); // 加速度计零偏协方差矩阵
  Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);// 陀螺仪零偏协方差矩阵
  // 设置一个初始的偏置噪声协方差矩阵，用于预积分过程中的初始偏置估计
  Matrix66 bias_acc_omega_init = I_6x6 * 1e-5;  // error in the bias used for preintegration

  // 设置预积分参数
  // MakeSharedD: NED坐标系，g默认为 9.81，这里设置为0.0; 
  // MakeSharedU: NEU坐标系，g默认为 9.81
  auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
  // PreintegrationBase params:
  p->accelerometerCovariance = measured_acc_cov;  // acc white noise in continuous 加速度计噪声协方差矩阵
  p->integrationCovariance = integration_error_cov;  // integration uncertainty continuous 积分误差协方差矩阵
  // should be using 2nd order integration
  // PreintegratedRotation params:
  p->gyroscopeCovariance = measured_omega_cov;  // gyro white noise in continuous 陀螺仪噪声协方差矩阵
  // PreintegrationCombinedMeasurements params:
  p->biasAccCovariance = bias_acc_cov;      // acc bias in continuous 加速度计偏置噪声协方差矩阵
  p->biasOmegaCovariance = bias_omega_cov;  // gyro bias in continuous 陀螺仪偏置噪声协方差矩阵
  p->biasAccOmegaInt = bias_acc_omega_init; // 初始偏置噪声协方差矩阵

  return p;
}

int main(int argc, char* argv[]) 
{
  string data_filename, output_filename;

  bool use_isam = false;

  po::variables_map var_map = parseOptions(argc, argv);

  data_filename = findExampleDataFile(var_map["data_csv_path"].as<string>());
  output_filename = var_map["output_filename"].as<string>();
  use_isam = var_map["use_isam"].as<bool>();

  // 初始化
  // 这里根据命令行参数use_isam的值，选择使用ISAM2优化器或Levenberg-Marquardt优化器。
  // ISAM2是一种增量平滑和映射算法，适用于大规模因子图，而Levenberg-Marquardt是一种基于梯度的优化算法。
  ISAM2* isam2 = 0;
  if (use_isam) 
  {
    printf("Using ISAM2\n");
    ISAM2Params parameters;
    parameters.relinearizeThreshold = 0.01;
    parameters.relinearizeSkip = 1;
    isam2 = new ISAM2(parameters);

  } 
  else 
  {
    printf("Using Levenberg Marquardt Optimizer\n");
  }

  // Set up output file for plotting errors
  FILE* fp_out = fopen(output_filename.c_str(), "w+");
  fprintf(fp_out, "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx,"
          "gt_qy,gt_qz,gt_qw\n");

  // Begin parsing the CSV file.  Input the first line for initialization.
  // From there, we'll iterate through the file and we'll preintegrate the IMU
  // or add in the GPS given the input.
  ifstream file(data_filename.c_str());
  string value;

  // 从CSV文件中读取初始状态，包括位置、姿态和速度。
  // 这个初始状态将作为因子图的起点。
  // Format is (N,E,D,qX,qY,qZ,qW,velN,velE,velD)
  Vector10 initial_state;
  getline(file, value, ',');  // i
  for (int i = 0; i < 9; i++) 
  {
    getline(file, value, ',');
    initial_state(i) = stof(value.c_str());
  }
  getline(file, value, '\n');
  initial_state(9) = stof(value.c_str());
  cout << "initial state:\n" << initial_state.transpose() << "\n\n";

  // 设置初始状态
  Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3),
                                         initial_state(4), initial_state(5));
  Point3 prior_point(initial_state.head<3>());
  Pose3 prior_pose(prior_rotation, prior_point); // 位置和姿态
  Vector3 prior_velocity(initial_state.tail<3>()); // 速度
  imuBias::ConstantBias prior_imu_bias;  // assume zero initial bias 零偏(假设为零)

  // 设置初始值: 给第一组状态(X0,V0,B0)设置初始值
  Values initial_values;
  int correction_count = 0;
  initial_values.insert(X(correction_count), prior_pose);
  initial_values.insert(V(correction_count), prior_velocity);
  initial_values.insert(B(correction_count), prior_imu_bias);

  // Assemble prior noise model and add it the graph.`
  // 定义噪声模型
  auto pose_noise_model = noiseModel::Diagonal::Sigmas(
      (Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5).finished());  // rad,rad,rad,m, m, m
  auto velocity_noise_model = noiseModel::Isotropic::Sigma(3, 0.1);  // m/s
  auto bias_noise_model = noiseModel::Isotropic::Sigma(6, 1e-3);

  // 创建一个非线性因子图对象，并向其中添加先验因子，同时附加了初始状态的不确定性。
  // 把先验因子数据关联到(X0, V0, B0)
  // Add all prior factors (pose, velocity, bias) to the graph.
  NonlinearFactorGraph* graph = new NonlinearFactorGraph();
  graph->addPrior(X(correction_count), prior_pose, pose_noise_model);
  graph->addPrior(V(correction_count), prior_velocity, velocity_noise_model);
  graph->addPrior(B(correction_count), prior_imu_bias, bias_noise_model);

  // 根据IMU参数和初始偏置创建预积分对象
  auto p = imuParams();
  std::shared_ptr<PreintegrationType> preintegrated = std::make_shared<PreintegratedImuMeasurements>(p, prior_imu_bias);

  assert(preintegrated);

  // 用变量的初始状态(先验值)初始化pre_state,pre_bias,pre_state  
  NavState prev_state(prior_pose, prior_velocity);
  imuBias::ConstantBias prev_bias = prior_imu_bias;
  NavState prop_state = prev_state;

  // Keep track of total error over the entire run as simple performance metric.
  // 记录位置和姿态误差(GPS与优化之后的位姿之差)
  double current_position_error = 0.0, current_orientation_error = 0.0;

  // 设置时间步长和输出时间。
  double output_time = 0.0;
  double dt = 0.005;  // The real system has noise, but here, results are nearly
                      // exactly the same, so keeping this for simplicity.

  // 读取数据文件中的每个测量值。
  // All priors have been set up, now iterate through the data file.
  while (file.good()) {
    // Parse out first value
    getline(file, value, ',');
    int type = stoi(value.c_str());

    // IMU的测量值
    if (type == 0) 
    {  // IMU measurement
      Vector6 imu;
      for (int i = 0; i < 5; ++i) 
      {
        getline(file, value, ',');
        imu(i) = stof(value.c_str());
      }
      getline(file, value, '\n');
      imu(5) = stof(value.c_str());

      // 将IMU测量值添加到预积分中
      // Adding the IMU preintegration.
      preintegrated->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);

    } 
    else if (type == 1) 
    {  // GPS measurement
      // 读取GPS测量值
      Vector7 gps;
      for (int i = 0; i < 6; ++i) 
      {
        getline(file, value, ',');
        gps(i) = stof(value.c_str());
      }
      getline(file, value, '\n');
      gps(6) = stof(value.c_str());

      // 增加校正计数(也可以认为增加关键帧计数)
      correction_count++;

      // Adding IMU factor and GPS factor and optimizing.
      auto preint_imu = dynamic_cast<const PreintegratedImuMeasurements&>(*preintegrated);

      // 这里创建了一个ImuFactor对象，它表示从一个关键帧到当前关键帧的IMU测量。
      // 这个因子使用了预积分的IMU测量（preint_imu），并且连接了两个连续关键帧的状态：
      // X(correction_count - 1)和V(correction_count - 1)分别代表上一个关键帧的姿态和速度。
      // X(correction_count)和V(correction_count)分别代表当前关键帧的姿态和速度。
      // B(correction_count - 1)代表上一个关键帧的IMU偏置。
      ImuFactor imu_factor(X(correction_count - 1), V(correction_count - 1),
                           X(correction_count), V(correction_count),
                           B(correction_count - 1), preint_imu);
      // 将刚刚创建的IMU因子添加到非线性因子图（graph）中。
      // 因子图是一个表示因子之间关系的图，这里的因子图用于存储和处理所有与状态估计相关的测量和约束。
      graph->add(imu_factor);

      // 这行代码创建了一个ConstantBias对象，它表示一个零偏置。
      // 在IMU数据融合中，偏置是随时间变化的，这里假设两个连续测量之间的偏置变化为零
      imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
      // 这行代码创建并添加了一个BetweenFactor因子，它约束了连续两个时间步的IMU偏置。
      // 这个因子表明我们期望correction_count - 1时刻的IMU偏置和correction_count时刻的IMU偏置之间没有变化，
      // 即偏置是恒定的。zero_bias是这个因子的测量值，而bias_noise_model是这个测量的噪声模型，它定义了偏置变化的不确定性。
      graph->add(BetweenFactor<imuBias::ConstantBias>(B(correction_count - 1), B(correction_count), 
                                                      zero_bias, bias_noise_model));

      // 添加GPS因子到因子图中。
      // GPS测量被封装为一个GPSFactor对象，并添加到因子图中。这个因子代表了GPS测量与估计的状态之间的差异
      // 这行代码创建了一个GPSFactor对象，它将GPS测量（Point3(gps(0), gps(1), gps(2))）与因子图中的状态变量X(correction_count)关联起来。
      // 这里correction_count是当前的校正计数，用于标识当前的状态变量。
      // GPS测量作为一个绝对参考，提供了对IMU相对测量的校正
      auto correction_noise = noiseModel::Isotropic::Sigma(3, 1.0);
      GPSFactor gps_factor(X(correction_count), // 键值
                           Point3(gps(0),   // N,
                                  gps(1),   // E,
                                  gps(2)),  // D,
                           correction_noise); // 噪声模型
      graph->add(gps_factor); // GPS加入因子图

      // Now optimize and compare results.这段代码的作用是根据IMU的预积分测量和上一个状态来预测当前的状态，并设置优化过程的初始值
      // 这行代码调用了preintegrated对象的predict方法，该方法用于根据上一个状态prev_state
      // 和上一个IMU偏置prev_bias来预测当前的状态。在GTSAM的框架中，预测通常是指使用传感器
      // 模型（在这里是IMU模型）来估计下一个时间步的状态。
      prop_state = preintegrated->predict(prev_state, prev_bias);
      // 这行代码将预测出的姿态prop_state.pose()插入到initial_values容器中，与键值X(correction_count)关联。
      // 在GTSAM中，Values对象用于存储变量的初始值或估计值，这里的X(correction_count)表示第correction_count
      // 个时间步的姿态变量。这个姿态值将作为优化算法的初始姿态估计。
      initial_values.insert(X(correction_count), prop_state.pose());
      // 这行代码将预测出的速度prop_state.v()插入到initial_values容器中，与键值V(correction_count)关联。
      // V(correction_count)表示第correction_count个时间步的速度变量。这个速度值将作为优化算法的初始速度估计。
      initial_values.insert(V(correction_count), prop_state.v());
      // 这行代码将上一个IMU偏置prev_bias插入到initial_values容器中，与键值B(correction_count)关联。
      // B(correction_count)表示第correction_count个时间步的IMU偏置变量。由于IMU偏置在预积分过程中通
      // 常是缓慢变化的，所以这里简单地将上一个时间步的偏置作为当前时间步的初始偏置估计。
      initial_values.insert(B(correction_count), prev_bias);


      // 这段代码根据是否使用ISAM优化器来选择不同的优化方法。如果使用ISAM，它将增量地更新状态估计；
      // 如果不使用ISAM，它将使用Levenberg-Marquardt优化器来解决整个因子图。
      // 优化的结果存储在result对象中，这个结果将被用于后续的状态估计和系统性能评估。
      Values result; // 存储优化过程的结果
      if (use_isam) 
      { 
        // 如果使用ISAM优化器，这行代码调用ISAM2对象的update方法，将因子图*graph和初始值initial_values传递给ISAM。
        // ISAM是一种增量平滑和映射算法，它在处理大规模因子图时非常有效，因为它只重新线性化和解决变化的部分，而不是整个图。
        isam2->update(*graph, initial_values);
        // 这行代码调用ISAM2对象的calculateEstimate方法，以计算状态变量的最新估计。这个估计将存储在result对象中。
        result = isam2->calculateEstimate();

        // reset the graph
        // 在ISAM优化完成后，这行代码将因子图的大小重置为0，有效地清除了图中的所有因子。
        // 这是因为ISAM在优化过程中已经对因子图进行了更新，所以不需要再次使用相同的因子。
        graph->resize(0);
        // 这行代码清除initial_values容器中的所有值。这是因为在ISAM优化后，
        // 初始值不再需要，并且为下一个时间步准备新的初始值。
        initial_values.clear();
      } 
      else 
      {
        // 这行代码创建了一个LevenbergMarquardtOptimizer对象，并将因子图*graph和初始值initial_values传递给它。
        // Levenberg-Marquardt优化器是一种基于梯度的优化算法，它结合了梯度下降和高斯-牛顿方法的优点。
        LevenbergMarquardtOptimizer optimizer(*graph, initial_values);
        // 这行代码调用LevenbergMarquardtOptimizer对象的optimize方法，以计算状态变量的最优估计。这个估计将存储在result对象中。
        // 优化过程的目标是找到一组状态变量的最优估计，使得所有因子图中的约束（测量）得到最佳满足。
        // 这个过程涉及到计算残差（测量值与预测值之间的差异）和雅可比矩阵（描述残差对每个状态变量的敏感度），
        // 然后迭代更新状态变量以最小化残差。这个估计将存储在result对象中。
        result = optimizer.optimize();
        // result变量存储了优化过程的结果，即状态变量的最优估计。
        // 这个结果包含了对当前帧（以及其他帧，取决于因子图的设计）的位姿、速度、IMU偏置等状态量的估计
      }

      // 这段代码是状态估计和传感器融合过程中的关键部分，它涉及到使用优化结果更新预积分对象、计算和报告位置与姿态误差

      // 1. 更新先前状态和偏置：
      // Overwrite the beginning of the preintegration for the next step.
      // 这行代码从优化结果result中提取出经过优化的姿态Pose3和速度Vector3，
      // 并将它们组合成一个新的NavState对象，代表先前的状态。
      // 这是为了在下一次迭代中使用优化后的状态作为新的起始点。
      prev_state = NavState(result.at<Pose3>(X(correction_count)),
                            result.at<Vector3>(V(correction_count)));
      // 这行代码从优化结果中提取出优化后的IMU偏置imuBias::ConstantBias，用于下一次预测的初始偏置。
      prev_bias = result.at<imuBias::ConstantBias>(B(correction_count));

      // 2. 重置预积分对象：
      // Reset the preintegration object.
      // 这行代码调用preintegrated对象的resetIntegrationAndSetBias方法，
      // 重置预积分过程并设置新的偏置。这是为了准备下一次的IMU数据预积分。
      preintegrated->resetIntegrationAndSetBias(prev_bias);

      // 3. 计算位置误差：

      // Print out the position and orientation error for comparison.
      // 提取优化后的状态中的位姿Pose3的平移部分Point3，即位置信息。
      Vector3 gtsam_position = prev_state.pose().translation();
      // 计算优化后的位置与GPS测量的位置之间的差值，gps.head<3>()提取GPS测量的位置部分。
      Vector3 position_error = gtsam_position - gps.head<3>();
      // 计算位置误差的范数（即欧几里得距离），作为位置误差的度量。
      current_position_error = position_error.norm();

      // 4. 计算姿态误差
      // 提取优化后的状态中的位姿Pose3的旋转部分Rot3，并转换为四元数Quaternion。
      Quaternion gtsam_quat = prev_state.pose().rotation().toQuaternion();
      // 根据GPS测量数据创建一个四元数，表示GPS测量的姿态。
      Quaternion gps_quat(gps(6), gps(3), gps(4), gps(5));
      //计算优化后的姿态与GPS姿态之间的相对旋转误差。这是通过将优化后的姿态与GPS姿态的逆相乘得到的。
      Quaternion quat_error = gtsam_quat * gps_quat.inverse();
      // 规范化四元数，确保其为单位四元数。
      quat_error.normalize();
      // 将四元数转换为欧拉角，这里假设小角度近似，即误差角的两倍即为欧拉角。
      Vector3 euler_angle_error(quat_error.x() * 2, quat_error.y() * 2,
                                quat_error.z() * 2);
      // 计算姿态误差的范数，作为姿态误差的度量
      current_orientation_error = euler_angle_error.norm();

      // 5. 输出统计信息
      // display statistics
      // 使用cout和fprintf输出当前的状态误差和GPS的对比数据，包括位置误差、
      // 姿态误差以及优化后的位置、姿态和原始GPS数据。这些信息有助于评估系统的定位精度和姿态估计的准确性。
      cout << "Position error:" << current_position_error << "\t "
           << "Angular error:" << current_orientation_error << "\n";

      fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
              output_time, gtsam_position(0), gtsam_position(1),
              gtsam_position(2), gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(),
              gtsam_quat.w(), gps(0), gps(1), gps(2), gps_quat.x(),
              gps_quat.y(), gps_quat.z(), gps_quat.w());

      // 6. 更新输出时间：
      // output_time += 1.0; 更新输出时间，通常用于记录处理每个数据点的时间间隔
      output_time += 1.0;

    } 
    else 
    {
      cerr << "ERROR parsing file\n";
      return 1;
    }
  }
  fclose(fp_out);
  cout << "Complete, results written to " << output_filename << "\n\n";

  return 0;
}
